package Basic;

import java.util.ArrayList;
import java.util.Comparator;
import java.util.PriorityQueue;

import Util.Util;

public class AStar {

	protected Tiles tiles;

	public AStar(Tiles tiles) {
		this.tiles = tiles;
	}

	protected static Comparator<Long> comparator = new Comparator<Long>() {

		@Override
		public int compare(Long o1, Long o2) {
			return (int) ((o1 & 0xFFFFFFFF) - (o2 & 0xFFFFFFFF));
		}
	};

	/**
	 * Chama o A* para dois pontos
	 * 
	 * @param start
	 *            (inicio do A*)
	 * @param goal
	 *            (objetivo do A*)
	 * @return Um vetor de pontos com o caminho INVERSO entre start e goal -
	 *         Para calcular o caminho correto basta mudar a ordem dos
	 *         parametros. Nao retorna (no caminho) o start e o goal.
	 */
	public ArrayList<Integer> run(int startPoint, int goalPoint) {
		return this.aStar(this.tiles.getTile(startPoint), this.tiles.getTile(goalPoint));
	}

	/**
	 * Calcula a distancia euclideana entre dois pontos
	 * 
	 * @param pSource
	 * @param pGoal
	 * @return A distancia euclideana entre p1 e p2
	 */
	public int euclidean(int pSource, int pGoal) {
		return Math.abs(Util.getX(pSource) - Util.getX(pGoal)) + Math.abs(Util.getY(pSource) - Util.getY(pGoal));
	}

	/**
	 * Calcula o A* para dois tiles
	 * 
	 * @param start
	 *            (inicio do A*)
	 * @param goal
	 *            (objetivo do A*)
	 * @return Um vetor de pontos com o caminho INVERSO entre start e goal -
	 *         Para calcular o caminho correto basta mudar a ordem dos
	 *         parametros. Nao retorna (no caminho) o start e o goal.
	 */
	private ArrayList<Integer> aStar(int start, int goal) {

		ArrayList<Integer> closedSet = new ArrayList<Integer>();
		// ArrayList<Integer> openSet = new ArrayList<Integer>();
		// BinaryHeap openSet = new BinaryHeap();
		PriorityQueue<Long> openSet = new PriorityQueue<Long>(100, comparator);

		int[] g = new int[this.tiles.totalTiles];
		int[] h = new int[this.tiles.totalTiles];
		int[] f = new int[this.tiles.totalTiles];
		boolean[] isInOpenSet = new boolean[this.tiles.totalTiles];
		boolean[] isInClosedSet = new boolean[this.tiles.totalTiles];
		int[] cameFrom = new int[this.tiles.totalTiles];
		for (int i = 0; i < cameFrom.length; i++) {
			cameFrom[i] = -1;
		}

		g[start] = 0;
		h[start] = this.euclidean(this.tiles.getCentral(start), this.tiles.getCentral(goal));
		f[start] = h[start];
		isInOpenSet[start] = true;
		openSet.add(Util.newTuple(start, f[start]));

		while (!openSet.isEmpty()) {

			// int x = openSet.remove().tile;
			int x = (int) (openSet.remove() >> 32);
			isInOpenSet[x] = false;

			closedSet.add(x);
			isInClosedSet[x] = true;

			ArrayList<Integer> neighbors = this.tiles.getNeighborsTiles(x);
			for (int i = 0; i < neighbors.size(); i++) {

				int y = neighbors.get(i);

				if (!isInClosedSet[y]) {

					int tentativeG = g[x] + (this.tiles.width / this.tiles.noTilesW);

					if (!isInOpenSet[y]) {
						cameFrom[y] = x;
						g[y] = tentativeG;
						h[y] = this.euclidean(this.tiles.getCentral(y), this.tiles.getCentral(goal));
						f[y] = g[y] + h[y];
						openSet.add(Util.newTuple(y, f[y]));
						isInOpenSet[y] = true;
					} else if (tentativeG < g[y]) {
						cameFrom[y] = x;
						g[y] = tentativeG;
						h[y] = this.euclidean(this.tiles.getCentral(y), this.tiles.getCentral(goal));
						f[y] = g[y] + h[y];
					}
				}
			}
		}

		ArrayList<Integer> ret = new ArrayList<Integer>();
		int iter = goal;
		boolean noPath = false;
		while (cameFrom[iter] != start) {
			if (cameFrom[iter] == -1) {
				noPath = true;
				break;
			}
			iter = cameFrom[iter];
			ret.add(this.tiles.getCentral(iter));
		}

		if (noPath) {
			return new ArrayList<Integer>();
		} else {
			return ret;
		}
	}
}
